#include "pid.h"
#include "calculate.h"
#include "math.h"

float kp_s = 6.0f;
float ki_s = 5.0f;
float kd_s = 3.0f;

float kp_a = 50.0f;
float ki_a = 5.0f;
float kd_a = 3.0f;

//pid参数初始化
void pid_init_s(pid_t *pid)
{
	pid->kp=kp_s;
	pid->ki=ki_s;
  pid->kd=kd_s;
}

void pid_init_a(pid_t *pid)
{
	pid->kp=kp_a;
	pid->ki=ki_a;
  pid->kd=kd_a;
}

//速度环
void pid_calculate(pid_t *pid,float target,float current)
{
  pid->target_value = target;
	pid->feedback_value = current;
	pid->err[1] = pid->err[0]; //上次误差
	pid->err[0] = pid->target_value - pid->feedback_value; //这次误差
	
	pid->p_out = pid->kp * pid->err[0]; //比例
	pid->i_out += pid->ki * pid->err[0]; //积分
	pid->d_out = pid->kd * (pid->err[0]-pid->err[1]);  //微分
	pid->out = pid->p_out + pid->i_out + pid->d_out;
	
	if(fabsf(pid->out) > 20000) {  // 接近输出限幅时
        pid->i_out *= 0.5f;        // 衰减积分项
    }

  Limit_Min_Max(pid->out, -25000,25000); //输出限幅
}
